import cv2
import numpy as np
import onnxruntime as ort
import time

# Config
model_path = "v5lite-s.onnx"
input_size = 320
conf_threshold = 0.4
nms_threshold = 0.5

# Load class names (replace with your own if needed)
class_names = [
    "person", "bicycle", "car", "motorbike", "aeroplane", "bus",
    "train", "truck", "boat", "traffic light", "fire hydrant", "stop sign",
    "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep",
    "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella",
    "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball",
    "kite", "baseball bat", "baseball glove", "skateboard", "surfboard",
    "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon",
    "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot",
    "hot dog", "pizza", "donut", "cake", "chair", "sofa", "pottedplant",
    "bed", "diningtable", "toilet", "tvmonitor", "laptop", "mouse", "remote",
    "keyboard", "cell phone", "microwave", "oven", "toaster", "sink",
    "refrigerator", "book", "clock", "vase", "scissors", "teddy bear",
    "hair drier", "toothbrush"
]



# Load model
session = ort.InferenceSession(model_path, providers=["CPUExecutionProvider"])
input_name = session.get_inputs()[0].name

def xywh2xyxy(x):
    y = np.copy(x)
    y[..., 0] = x[..., 0] - x[..., 2] / 2
    y[..., 1] = x[..., 1] - x[..., 3] / 2
    y[..., 2] = x[..., 0] + x[..., 2] / 2
    y[..., 3] = x[..., 1] + x[..., 3] / 2
    return y

def postprocess(prediction):
    boxes = []
    confidences = []
    class_ids = []

    pred = prediction[0]
    pred = pred[pred[..., 4] > conf_threshold]

    for det in pred:
        scores = det[5:]
        class_id = np.argmax(scores)
        confidence = scores[class_id]
        if confidence > conf_threshold:
            box = det[0:4] * np.array([input_size, input_size, input_size, input_size])
            box = xywh2xyxy(box)
            boxes.append(box.astype(np.int32))
            confidences.append(float(confidence))
            class_ids.append(class_id)

    indices = cv2.dnn.NMSBoxes(boxes, confidences, conf_threshold, nms_threshold)
    results = []
    if indices is not None:
        for i in indices:
            i = i[0] if isinstance(i, (tuple, list, np.ndarray)) else i
            results.append((boxes[i], confidences[i], class_ids[i]))
    return results

cap = cv2.VideoCapture(0)
assert cap.isOpened(), "Cannot open camera"

while True:
    ret, frame = cap.read()
    if not ret:
        break

    h, w, _ = frame.shape
    img = cv2.resize(frame, (input_size, input_size))
    img_input = img.astype(np.float32) / 255.0
    img_input = np.transpose(img_input, (2, 0, 1))
    img_input = np.expand_dims(img_input, axis=0)

    start = time.time()
    output = session.run(None, {input_name: img_input})[0]
    end = time.time()

    detections = postprocess(output)

    scale_x = w / input_size
    scale_y = h / input_size

    for box, conf, cls in detections:
        x1 = int(box[0] * scale_x)
        y1 = int(box[1] * scale_y)
        x2 = int(box[2] * scale_x)
        y2 = int(box[3] * scale_y)

        label_text = f"{class_names[cls] if cls < len(class_names) else cls}: {conf:.2f}"
        color = (0, 255, 0)

        cv2.rectangle(frame, (x1, y1), (x2, y2), color, 2)

        (text_w, text_h), baseline = cv2.getTextSize(label_text, cv2.FONT_HERSHEY_SIMPLEX, 0.6, 2)
        label_origin = (x1, y1 - 10 if y1 - 10 > 10 else y1 + text_h + 10)

        cv2.rectangle(frame,
                      (label_origin[0], label_origin[1] - text_h - baseline),
                      (label_origin[0] + text_w, label_origin[1] + baseline),
                      color, thickness=cv2.FILLED)

        cv2.putText(frame, label_text, label_origin,
                    cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 0), 2)

    fps = 1 / (end - start + 1e-6)
    cv2.putText(frame, f"FPS: {fps:.2f}", (10, 25), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 0), 2)

    cv2.imshow("YOLOv5-Lite ONNX", frame)
    if cv2.waitKey(1) & 0xFF == ord("q"):
        break

cap.release()
cv2.destroyAllWindows()
